#include <ModbusMaster.h>
#include "motor.h"

#define MOTOR_REGISTER (0x20)//address
#define MOTOR_DATA_LEN (23)  //data length
#define ENCODER_LOW    (5)

#define WHEEL_TRACK  (0.33)
#define WHEEL_RADIUS (0.0625)
#define FACTOR_30K   (0.00825)

static ModbusMaster modbus;

static double rpmToValue(double rpm)
{
  return rpm / FACTOR_30K;
}

static double linearToRpm(double lin)
{
  double mileage = lin * 60.0;//mileage in one minute
  double rpm = mileage / (2.0 * M_PI * WHEEL_RADIUS) / 1000.0;// RPM = mileage / (2*pi*radius)
  
  return rpm;
}


void init()
{
  Serial2.begin(115200);
  modbus.begin(0, Serial2);
}
bool getEncoder(int id, int32_t &encoder)
{
  uint8_t  result;
  uint16_t data[MOTOR_DATA_LEN];
//  uint16_t tmp = data[ENCODER_LOW];

  id ++;
  modbus.setSlaveID(id);

  result = modbus.readHoldingRegisters(MOTOR_REGISTER, MOTOR_DATA_LEN);  
  if (result == modbus.ku8MBSuccess) {
    for (int j = 0; j < MOTOR_DATA_LEN; j++) {
      data[j] = modbus.getResponseBuffer(j);
    }
//    int32_t tmp = (int32_t)data[ENCODER_LOW];
    encoder = (((int32_t)data[4]) << 16) | ((int32_t)data[ENCODER_LOW]);
    if (1 == id) encoder = -encoder;
    return true;
  }

  return false;
}
bool move(int id, int speed)
{
  id ++;
  modbus.setSlaveID(id);
  modbus.writeSingleRegister(0x43, (long)rpmToValue(linearToRpm((double)(speed))));
}

